/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#ifndef COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_

#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/InflationPluginConfig.h>
#include <dynamic_reconfigure/server.h>
#include <boost/thread.hpp>

namespace costmap_2d
{

#ifdef SEMANTIC
static const int HIGH_INFLATION_SHIFT_GRID = 2;
#endif

/**
* @class CellData  用来记录当前cell的索引和与它最近的障碍物的索引的
* @brief Storage for cell information used during obstacle inflation
* 首先定义一个CellData类存储在障碍物膨胀时使用的单元信息，包括单元格在costmap中的索引，costmap中的坐标x,y,costmap中最近障碍物单元的x,y坐标。
*/
class CellData
{
public:
    /**
     * @brief  Constructor for a CellData objects
     * @param  i The index of the cell in the cost map
     * @param  x The x coordinate of the cell in the cost map
     * @param  y The y coordinate of the cell in the cost map
     * @param  sx The x coordinate of the closest obstacle cell in the costmap
     * @param  sy The y coordinate of the closest obstacle cell in the costmap
     * @return
     */
    CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
            index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
#ifdef SEMANTIC
    CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy, uint8_t option) :
            index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy), option_(option) {}

    uint8_t option_;
#endif
    // costmap中单元格的索引
    unsigned int index_;
    // 单元格cell在costmap中的坐标x,y
    unsigned int x_, y_;
    // costmap中最近障碍物单元的x,y坐标
    unsigned int src_x_, src_y_;
};
// inflation_layer作为一个地图层，具备基本的更新边界和更新代价的功能，最重要的功能是设置膨胀区域,为此新增了栅格单元类。
class InflationLayer : public Layer
{
public:
    InflationLayer();

    virtual ~InflationLayer()
    {
        deleteKernels();
        delete dsrv_;
        delete[] seen_;
    }

    virtual void onInitialize();
    // need_reinflation_默认false，更新bound这里和其他两层的主要区别是，膨胀层在传入的bound的值的基础上，通过inflation_radius_再次扩张
    virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y,
                              double *max_x, double *max_y);
    // 用指针master_array指向主地图，并获取主地图的尺寸，确认seen_数组被正确设置。
    virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j);

    virtual bool isDiscretized() { return true; }

    virtual void matchSize();

    virtual void reset() { onInitialize(); }

    /** @brief  Given a distance, compute a cost. 根据距离计算代价值，大概分三个情况:
     * @param  distance The distance from an obstacle in cells
     * 1. 如果distance为0，即(0,0)点，则设置cached_costs_上的值为LETHAL_OBSTACLE(254)，表示为障碍物本身；
     * 2. 如果distance ≤ 机器人足迹内切圆半径，设置值为INSCRIBED_INFLATED_OBSTACLE（253），即由于机器人有体积造成的障碍物膨胀；
     * 3. 如果机器人足迹内切圆半径 < distance ≤ cell_inflation_radius_，则以距离远近为比例（指数型）设置值。
     * @param  distance 障碍物单元格的距离
     * @return 对于这个距离的代价值
     * @return A cost value for the distance
    */
    virtual inline unsigned char computeCost(double distance) const
    {
        unsigned char cost = 0;
#ifdef SEMANTIC
        if (distance == 0) cost = XJU_COST_LETHAL_OBSTACLE;
        else if (distance * resolution_ <= inscribed_radius_) cost = XJU_COST_INSCRIBED_INFLATED_OBSTACLE;
        else
        {
            // make sure cost falls off by Euclidean distance 确保成本以欧几里德距离递减
            double euclidean_distance = distance * resolution_;
            double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
            cost = (unsigned char)((XJU_COST_INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
        }
#else
        if (distance == 0) cost = LETHAL_OBSTACLE;
        else if (distance * resolution_ <= inscribed_radius_) cost = INSCRIBED_INFLATED_OBSTACLE;
        else
        {
            // make sure cost falls off by Euclidean distance 确保成本以欧几里德距离递减
            double euclidean_distance = distance * resolution_;
            double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
            cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
        }
#endif
        return cost;
    }

    /**
     * @brief Change the values of the inflation radius parameters
     * @param inflation_radius The new inflation radius
     * @param cost_scaling_factor The new weight
     */
    void setInflationParameters(double inflation_radius, double cost_scaling_factor);

protected:
    virtual void onFootprintChanged();

    boost::recursive_mutex *inflation_access_;

    double resolution_;
    double inflation_radius_;  // 膨胀半径 单位：m
    double inscribed_radius_;  // 内接半径
    double weight_;
    bool inflate_unknown_;     // 未知区域是否膨胀

private:
    /** 找到当前cell与最近的障碍物cell的距离
     * @brief  Lookup pre-computed distances
     * @param mx The x coordinate of the current cell
     * @param my The y coordinate of the current cell
     * @param src_x The x coordinate of the source cell
     * @param src_y The y coordinate of the source cell
     * @return
     */
    inline double distanceLookup(int mx, int my, int src_x, int src_y)
    {
        unsigned int dx = abs(mx - src_x);
        unsigned int dy = abs(my - src_y);
        return cached_distances_[dx][dy];
    }

    /** 通过该cell与最近障碍物的距离来确定该cell的cost
     * @brief  Lookup pre-computed costs 查找预先计算的代价
     * @param mx The x coordinate of the current cell
     * @param my The y coordinate of the current cell
     * @param src_x The x coordinate of the source cell
     * @param src_y The y coordinate of the source cell
     * @return
     */
    inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
    {
        unsigned int dx = abs(mx - src_x);
        unsigned int dy = abs(my - src_y);
        return cached_costs_[dx][dy];
    }

#ifdef SEMANTIC
    inline unsigned char costLookup(int mx, int my, int src_x, int src_y, uint8_t option)
    {
        unsigned int dx = abs(mx - src_x);
        unsigned int dy = abs(my - src_y);
        switch (option)
        {
            case XJU_OPTION_NONE: return XJU_COST_FREE_SPACE;
            case XJU_OPTION_LESS:
                return (cached_costs_[dx][dy] >= XJU_COST_INSCRIBED_INFLATED_OBSTACLE) ?
                cached_costs_[dx][dy] : XJU_COST_FREE_SPACE;
            case XJU_OPTION_MORE:
            {
                auto radius = cached_distances_[dx][dy];
                if (radius <= HIGH_INFLATION_SHIFT_GRID) return XJU_COST_INSCRIBED_INFLATED_OBSTACLE;
                else
                {
                    auto x = static_cast<unsigned int>(dx * (1 - HIGH_INFLATION_SHIFT_GRID / radius));
                    auto y = static_cast<unsigned int>(dy * (1 - HIGH_INFLATION_SHIFT_GRID / radius));
                    return std::min(cached_costs_[x][y], XJU_COST_INSCRIBED_INFLATED_OBSTACLE);
                }
            }
            default: return cached_costs_[dx][dy];
        }
    }
#endif
    void computeCaches();

    void deleteKernels();

    void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char *master_grid);

    // 计算栅格距离: 直接调用LayeredCostmap实例化的对象layered_costmap_的cellDistance()函数
    // 把global系以米为单位的长度转换成以cell为单位的距离，故获得了地图上的膨胀参数cell_inflation_radius_。
    unsigned int cellDistance(double world_dist)
    {
        return layered_costmap_->getCostmap()->cellDistance(world_dist);
    }

    // 给定代价地图中单元格的索引，将其放入等待障碍物膨胀的列表中
    inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
                        unsigned int src_x, unsigned int src_y);
#ifdef SEMANTIC
    inline void enqueue(costmap_2d::Costmap2D& grid, unsigned int index, unsigned int mx, unsigned int my,
                        unsigned int src_x, unsigned int src_y, uint8_t option);

    inline bool inflateCell(costmap_2d::Costmap2D const& master_grid, unsigned int& index,
                            unsigned int i, unsigned int j) const;
#endif
    unsigned int cell_inflation_radius_;
    unsigned int cached_cell_inflation_radius_;
    std::map<double, std::vector<CellData>> inflation_cells_;

    int seen_size_;
    bool *seen_;             // 用于标记cell是否已经过计算
    bool need_reinflation_;  // 指示下次应该重新膨胀整个代价地图。
    unsigned char **cached_costs_; // 缓存的代价
    double **cached_distances_;    // 缓存的距离
    double last_min_x_, last_min_y_, last_max_x_, last_max_y_;

    dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;

    void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);

};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_INFLATION_LAYER_H_
